package team240;

import java.util.Stack;
import battlecode.common.*;

public class Robots {
	//Put methods that all robots need in here
	public enum Status { 
		COMBAT, 
		MINE, 
		TRAVEL, 
		DEFEND,
		SEARCH,
		FINDHOME
	}
	
	RobotController myRC;
	Status myStatus;
	int straightPatience; 
	int diagonalPatience;
	int sidePatience;
	int backPatience;
	Stack<MapLocation> myPath;
	final double aStarAlpha = 2;
	public Robots(RobotController inRC) {
		myRC = inRC;
		myPath = new Stack<MapLocation>();
	}
	public void runRobots() {
      while(true) {
         try {
        	 if (myRC.getRobotType().isAirborne()) {
        		 AirRobot myAirRobot = new AirRobot(myRC);
        		 myAirRobot.runAirRobot();
        	 }
        	 else {
        		 GroundRobot myGroundRobot = new GroundRobot(myRC);
        		 myGroundRobot.runGroundRobot();
        	 }
         }catch(Exception e) {
            System.out.println("caught exception:");
            e.printStackTrace();
         }
      }
   }
	public MapLocation getClosestLocation(MapLocation[] choices) {
		//returns the closest location to the calling robot within a given array
		//make sure that the input array has at least one MapLocation
		MapLocation myLocation = myRC.getLocation();
		MapLocation closest = choices[0];
		int lowest = getDistanceSquared(myLocation,closest);
		for (int k=1;k<choices.length;k++) {
			MapLocation newLocation = choices[k];
			int newDistance = getDistanceSquared(myLocation,closest);
			if (newDistance < lowest) {
				closest = newLocation;
				lowest = newDistance;
			}
		}
		return closest;
	}
	public int getDistanceSquared(MapLocation start, MapLocation goal) {
		//returns the squared distance between two locations
		int startX = start.getX();
		int startY = start.getY();
		int goalX = goal.getX();
		int goalY = goal.getY();
		return ((goalX-startX)*(goalX-startX)+(goalY-startY)*(goalY-startY));
	}
	public Direction getDirection(MapLocation start,MapLocation goal) {
		//returns the closest direction given two MapLocations
		double startX = (double) start.getX();
		double startY = (double) start.getY();
		double goalX = (double) goal.getX();
		double goalY = (double) goal.getY();
		double diffY = goalY-startY;
		double diffX = goalX-startX;
		if(diffY == 0) {
			if(diffX > 0) {
				return Direction.EAST;
			}
			else {
				return Direction.WEST;
			}
		}
		else {
			if(diffX == 0) {
				if(diffY < 0) {
					return Direction.NORTH;
				}
				else {
					return Direction.SOUTH;
				}
			}
			else {
				double innerAngle = Math.atan(Math.abs(diffY)/Math.abs(diffX));
				if(diffY < 0) {
					if(diffX > 0) {
						if(innerAngle <= Math.PI/8) {
							return Direction.EAST;
						}
						else {
							if(innerAngle >= 3*(Math.PI/8)) {
								return Direction.NORTH;
							}
							else {
								return Direction.NORTH_EAST;
							}
						}
					}
					else {
						if(innerAngle <= Math.PI/8) {
							return Direction.WEST;
						}
						else {
							if(innerAngle >= 3*(Math.PI/8)) {
								return Direction.NORTH;
							}
							else {
								return Direction.NORTH_WEST;
							}
						}
					}
				}
				else {
					if(diffX > 0) {
						if(innerAngle <= Math.PI/8) {
							return Direction.EAST;
						}
						else {
							if(innerAngle >= 3*(Math.PI/8)) {
								return Direction.SOUTH;
							}
							else {
								return Direction.SOUTH_EAST;
							}
						}
					}
					else {
						if(innerAngle <= Math.PI/8) {
							return Direction.WEST;
						}
						else {
							if(innerAngle >= 3*(Math.PI/8)) {
								return Direction.SOUTH;
							}
							else {
								return Direction.SOUTH_WEST;
							}
						}
					}
				}
			}
		}
	}
	public Robot findNearestEnemy(Robot[] airRobots,Robot[] groundRobots) {
		Robot nearestEnemy = null;
		try {
			//returns the nearest enemy robot within sensible range
			//returns null if there are no enemies within sensor range
			Team myTeam = myRC.getTeam();
			MapLocation myLocation = myRC.getLocation();
			int closest = 100;
			for (Robot x: groundRobots) {
				RobotInfo foreignRobot = myRC.senseRobotInfo(x);
				if (foreignRobot.team != myTeam) {
					int distanceToRobot = getDistanceSquared(myLocation,foreignRobot.location);
					if (distanceToRobot < closest) {
						nearestEnemy = x;
						closest = distanceToRobot;
					}
				}
			}
			for (Robot y: airRobots) {
				RobotInfo foreignRobot = myRC.senseRobotInfo(y);
				if (foreignRobot.team != myTeam) {
					int distanceToRobot = getDistanceSquared(myLocation,foreignRobot.location);
					if (distanceToRobot < closest) {
						nearestEnemy = y;
						closest = distanceToRobot;
					}
				}
			}
			return nearestEnemy;
		} catch(Exception e) {
            System.out.println("caught exception:");
            e.printStackTrace();
            return nearestEnemy;
        }
	}
	public Robot findNearestEnemyAndTransferEnergon(Robot[] airRobots,Robot[] groundRobots) {
		Robot nearestEnemy = null;
		try {
			//returns the nearest enemy robot within sensible range
			//returns null if there are no enemies within sensor range
			Team myTeam = myRC.getTeam();
			MapLocation myLocation = myRC.getLocation();
			int closest = 100;
			for (Robot x: groundRobots) {
				RobotInfo foreignRobot = myRC.senseRobotInfo(x);
				if (foreignRobot.team != myTeam) {
					int distanceToRobot = getDistanceSquared(myLocation,foreignRobot.location);
					if (distanceToRobot < closest) {
						nearestEnemy = x;
						closest = distanceToRobot;
					}
				}
				else {
					if (foreignRobot.location.isAdjacentTo(myLocation)) {
						if (foreignRobot.energonReserve < 3) {
							double amountToTransfer = myRC.getEnergonLevel()-foreignRobot.eventualEnergon*(myRC.getRobotType().maxEnergon()/foreignRobot.type.maxEnergon())/2;
							if (amountToTransfer > 0) {
								if (amountToTransfer < 10) {
									myRC.transferEnergon(amountToTransfer, foreignRobot.location, RobotLevel.ON_GROUND);
								}
								else {
									myRC.transferEnergon(10, foreignRobot.location, RobotLevel.ON_GROUND);
								}
							}
						}
					}
				}
			}
			for (Robot y: airRobots) {
				RobotInfo foreignRobot = myRC.senseRobotInfo(y);
				if (foreignRobot.team != myTeam) {
					int distanceToRobot = getDistanceSquared(myLocation,foreignRobot.location);
					if (distanceToRobot < closest) {
						nearestEnemy = y;
						closest = distanceToRobot;
					}
				}
			}
			return nearestEnemy;
		} catch(Exception e) {
            System.out.println("caught exception:");
            e.printStackTrace();
            return nearestEnemy;
        }
	}
	public double getMovementPenalty(MapLocation start, MapLocation finish) {
		//returns the terrain height movement penalty for moving from one location to another, excluding unit movement delay
		try {
			//hopefully comment this out if it is never used
			if (myRC.canSenseSquare(start) && myRC.canSenseSquare(finish)) {
				double delta = myRC.senseHeightOfLocation(finish)-myRC.senseHeightOfLocation(start);
				if (Math.abs(delta) <= 1) {
					return 0;
				}
				else {
					if (delta > 1) {
						return GameConstants.CLIMBING_PENALTY_RATE*delta*delta;
					}
					else {
						return GameConstants.FALLING_PENALTY_RATE*delta;
					}
				}
			}
			else {
				System.out.println("You tried to get the movement cost between squares you couldn't sense");
				return 999999;
			}
		} catch(Exception e) {
            System.out.println("caught exception:");
            e.printStackTrace();
            return 999999;
        }
	}
	public boolean messageValidated(Message newMessage) {
		boolean validated = false;
		if (newMessage.ints.length > 1) {
			if (Clock.getRoundNum()-newMessage.ints[1] < 2) {
				validated = true;
			}
		}
		return validated;
	}
	public void getShortestSensiblePath(MapLocation goal) {
		try {
			//gets the shortest path between your location and a goal using A*
			int myOrthogonalDelay = myRC.getRobotType().moveDelayOrthogonal();
			int myDiagonalDelay = myRC.getRobotType().moveDelayDiagonal();
			MapLocation currentLocation = myRC.getLocation();
			Direction lookDirection = null;
			int waterCounter = 0;
			switch (myRC.getRobotType()) {
			case WORKER: lookDirection = Direction.NORTH; waterCounter = 8; break;
			case SOLDIER: lookDirection = myRC.getDirection().rotateLeft().rotateLeft(); waterCounter = 5; break;
			case CANNON: lookDirection = myRC.getDirection().rotateLeft(); waterCounter = 3; break;
			default: //error unexpected robot type
			}
			MapLocation newLocation = currentLocation.add(lookDirection);
			MapNode closed = new MapNode(currentLocation,0,0,null);
			while (myRC.senseTerrainTile(newLocation).getType() != TerrainTile.TerrainType.LAND) {
				MapNode makeClosed = new MapNode(newLocation,0,0,null);
				closed.pre = makeClosed;
				makeClosed.next = closed;
				closed = makeClosed;
				lookDirection = lookDirection.rotateRight();
				newLocation = currentLocation.add(lookDirection);
				waterCounter--;
				if (waterCounter == 0) {
					System.out.println("Infinite loop in getShortestSendiblePath avoided. Please tell Beecher if you are reading this.");
					return;
				}
			}
			MapNode open;
			if (lookDirection.isDiagonal()) {
				open = new MapNode(newLocation,getMovementPenalty(currentLocation,newLocation)+myOrthogonalDelay,guessDistanceToGoal(currentLocation,newLocation,myDiagonalDelay),closed);
			}
			else {
				open = new MapNode(newLocation,getMovementPenalty(currentLocation,newLocation)+myOrthogonalDelay,guessDistanceToGoal(currentLocation,newLocation,myOrthogonalDelay),closed);
			}
			for (int k=1;k<waterCounter;k++) {
				lookDirection = lookDirection.rotateRight();
				newLocation = currentLocation.add(lookDirection);
				if (myRC.senseTerrainTile(newLocation).getType() == TerrainTile.TerrainType.LAND) {
					if (lookDirection.isDiagonal()) {
						MapNode newNode = new MapNode(newLocation,getMovementPenalty(currentLocation,newLocation)+myOrthogonalDelay,guessDistanceToGoal(currentLocation,newLocation,myDiagonalDelay),closed);
						open = addNode(open,newNode,closed);
					}
					else {
						MapNode newNode = new MapNode(newLocation,getMovementPenalty(currentLocation,newLocation)+myOrthogonalDelay,guessDistanceToGoal(currentLocation,newLocation,myOrthogonalDelay),closed);
						open = addNode(open,newNode,closed);
					}
				}
				else {
					MapNode makeClosed = new MapNode(newLocation,0,0,null);
					closed.pre = makeClosed;
					makeClosed.next = closed;
					closed = makeClosed;
				}
			}
			MapLocation openLocation = open.myLocation;
			double distanceToOpen = open.myDistance;
			while (openLocation != goal) {
				lookDirection = Direction.NORTH;
				for (int m=1;m<3;m++) {
					newLocation = openLocation.add(lookDirection);
					if (myRC.canSenseSquare(newLocation)) {
						if (myRC.senseTerrainTile(newLocation).getType() == TerrainTile.TerrainType.LAND) {
							MapNode newNode = new MapNode(newLocation,distanceToOpen+getMovementPenalty(openLocation,newLocation)+myOrthogonalDelay,guessDistanceToGoal(openLocation,newLocation,myOrthogonalDelay),open);
							open = addNode(open,newNode,closed);
						}
					}
					newLocation = openLocation.subtract(lookDirection);
					if (myRC.canSenseSquare(newLocation)) {
						if (myRC.senseTerrainTile(newLocation).getType() == TerrainTile.TerrainType.LAND) {
							MapNode newNode = new MapNode(newLocation,distanceToOpen+getMovementPenalty(openLocation,newLocation)+myOrthogonalDelay,guessDistanceToGoal(openLocation,newLocation,myOrthogonalDelay),open);
							open = addNode(open,newNode,closed);
						}
					}
					lookDirection = lookDirection.rotateRight();
					newLocation = openLocation.add(lookDirection);
					if (myRC.canSenseSquare(newLocation)) {
						if (myRC.senseTerrainTile(newLocation).getType() == TerrainTile.TerrainType.LAND) {
							MapNode newNode = new MapNode(newLocation,distanceToOpen+getMovementPenalty(openLocation,newLocation)+myDiagonalDelay,guessDistanceToGoal(openLocation,newLocation,myOrthogonalDelay),open);
							open = addNode(open,newNode,closed);
						}
					}
					newLocation = openLocation.subtract(lookDirection);
					if (myRC.canSenseSquare(newLocation)) {
						if (myRC.senseTerrainTile(newLocation).getType() == TerrainTile.TerrainType.LAND) {
							MapNode newNode = new MapNode(newLocation,distanceToOpen+getMovementPenalty(openLocation,newLocation)+myDiagonalDelay,guessDistanceToGoal(openLocation,newLocation,myOrthogonalDelay),open);
							open = addNode(open,newNode,closed);
						}
					}
					lookDirection = lookDirection.rotateRight();
				}
				closed.pre = open;
				if (open.next == null) {
					return;
				}
				open = open.next;
				openLocation = open.myLocation;
				distanceToOpen = open.myDistance;
			}
			myPath.push(goal);
			MapNode traceNode = open.previousNode;
			MapLocation traceLocation = traceNode.myLocation;
			while (traceLocation != currentLocation) {
				myPath.push(traceLocation);
				traceNode = traceNode.previousNode;
				traceLocation = traceNode.myLocation;
			}
		} catch(Exception e) {
            System.out.println("caught exception:");
            e.printStackTrace();
        }
	}
	public double guessDistanceToGoal (MapLocation start, MapLocation goal, int moveDelay) {
		//returns an overestimated guess of the distance between two locations to make A* movement faster
		return (Math.abs(goal.getX()-start.getX())+Math.abs(goal.getY()-start.getY())*moveDelay*aStarAlpha);
	}
	public MapNode addNode(MapNode openSet,MapNode newNode,MapNode closedSet) {
		//adds a new neighbor node to the open set if it hasn't been checked already
		MapLocation newLocation = newNode.myLocation;
		while (closedSet.next != null){
			if (closedSet.myLocation == newLocation) {
				if (newNode.myDistance+getMovementPenalty(newLocation,closedSet.myLocation)+myRC.getRobotType().moveDelayDiagonal() < closedSet.myDistance) {
					//comment this out
					if (closedSet.pre != null) {
						closedSet.pre.next = closedSet.next;
						closedSet.next.pre = closedSet.pre;
					}
					else {
						closedSet.next.pre = null;
					}
					System.out.println("A* just pulled a node out of the closed set. Tell Beecher if you read this.");
				}
				else {
					return openSet;
				}
			}
			else {
				closedSet = closedSet.next;
			}
		}
		if (closedSet.myLocation == newLocation) {
			if (newNode.myDistance+getMovementPenalty(newLocation,closedSet.myLocation)+myRC.getRobotType().moveDelayDiagonal() < closedSet.myDistance) {
				//comment this out
				if (closedSet.pre != null) {
					closedSet.pre.next = null;
				}
				else {
					System.out.println("A* just pulled the last node out of the closed set. Tell Beecher if you read this.");
				}
				System.out.println("A* just pulled a node out of the closed set. Tell Beecher if you read this.");
			}
			else {
				return openSet;
			}
		}
		else {
			MapNode current = openSet;
			double currentTotal = current.myTotal;
			double newTotal = newNode.myTotal;
			while (currentTotal<=newTotal && current.next!=null) {
				if (current.myLocation == newLocation) {
					return openSet;
				}
				else {
					current = current.next;
				}
			}
			if (current.next == null) {
				if (current.myLocation != newLocation) {
					if (currentTotal<=newTotal) {
						current.next = newNode;
						newNode.pre = current;
						return openSet;
					}
					else {
						if (current.pre == null) {
							current.pre = newNode;
							newNode.next = current;
							return newNode;
						}
						else {
							newNode.pre = current.pre;
							newNode.next = current;
							newNode.pre.next = newNode;
							current.pre = newNode;
							return openSet;
						}
					}
				}
				else {
					if (current.pre == null) {
						return newNode;
					}
					else {
						newNode.pre = current.pre;
						current.pre = newNode;
						return openSet;
					}
				}
			}
			else {
				newNode.next = current;
				if (current.pre == null) {
					current.pre = newNode;
					while (current.next != null) {
						if (current.myLocation == newLocation) {
							current.pre.next = current.next;
							current.next.pre = current.pre;
						}
					}
					if (current.myLocation == newLocation) {
						current.pre.next = null;
					}
					return newNode;
				}
				else {
					current.pre.next = newNode;
					current.pre = newNode;
					while (current.next != null) {
						if (current.myLocation == newLocation) {
							current.pre.next = current.next;
							current.next.pre = current.pre;
						}
					}
					if (current.myLocation == newLocation) {
						current.pre.next = null;
					}
					return openSet;
				}
			}
		}
		return openSet;
	}
	public void moveOneInDirection(Direction heading) {
		//moves one step in the desired direction
		//used for short movement where intelligence and looking ahead are not needed
		//you need to check if movement is active before calling this method
		try {
			//possibility: implement "moveForward" here. after setting direction, moveForward+= any number. before executing any movement code, check to see if moveForward==0. if not, check to see if you can move forward and then moveForward--
			if (myRC.canMove(heading)) {
				if (myRC.getDirection()==heading) {
					myRC.moveForward();
					resetPatience();
				}
				else if (myRC.getDirection()==heading.opposite()) {
					myRC.moveBackward();
					resetPatience();
				}
				else {
					myRC.setDirection(heading);
				}
			}
			else {
				if (straightPatience == 0) {
					if (myRC.canMove(heading.rotateLeft())) {
						if (myRC.getDirection()==heading.rotateLeft()) {
							myRC.moveForward();
							resetPatience();
						}
						else if (myRC.getDirection()==heading.opposite().rotateLeft()) {
							myRC.moveBackward();
							resetPatience();
						}
						else {
							myRC.setDirection(heading.rotateLeft());
						}
					}
					else if (myRC.canMove(heading.rotateRight())) {
						if (myRC.getDirection()==heading.rotateRight()) {
							myRC.moveForward();
							resetPatience();
						}
						else if (myRC.getDirection()==heading.opposite().rotateRight()) {
							myRC.moveBackward();
							resetPatience();
						}
						else {
							myRC.setDirection(heading.rotateRight());
						}
					}
					else if (diagonalPatience == 0) {
						if (myRC.canMove(heading.rotateLeft().rotateLeft())) {
							if (myRC.getDirection()==heading.rotateLeft().rotateLeft()) {
								myRC.moveForward();
								resetPatience();
							}
							else if (myRC.getDirection()==heading.opposite().rotateLeft().rotateLeft()) {
								myRC.moveBackward();
								resetPatience();
							}
							else {
								myRC.setDirection(heading.rotateLeft().rotateLeft());
							}
						}
						else if (myRC.canMove(heading.rotateRight().rotateRight())) {
							if (myRC.getDirection()==heading.rotateRight().rotateRight()) {
								myRC.moveForward();
								resetPatience();
							}
							else if (myRC.getDirection()==heading.opposite().rotateRight().rotateRight()) {
								myRC.moveBackward();
								resetPatience();
							}
							else {
								myRC.setDirection(heading.rotateRight().rotateRight());
							}
						}
						else if (sidePatience == 0) {
							if (myRC.canMove(heading.opposite().rotateRight())) {
								if (myRC.getDirection()==heading.rotateRight()) {
									myRC.moveBackward();
									resetPatience();
								}
								else if (myRC.getDirection()==heading.opposite().rotateRight()) {
									myRC.moveForward();
									resetPatience();
								}
								else {
									myRC.setDirection(heading.opposite().rotateRight());
								}
							}
							else if (myRC.canMove(heading.opposite().rotateLeft())) {
								if (myRC.getDirection() == heading.rotateLeft()) {
									myRC.moveBackward();
									resetPatience();
								}
								else if (myRC.getDirection() == heading.opposite().rotateLeft()) {
									myRC.moveForward();
									resetPatience();
								}
								else {
									myRC.setDirection(heading.opposite().rotateLeft());
								}
							}
							else if (backPatience == 0) {
								if (myRC.canMove(heading.opposite())) {
									if(myRC.getDirection() == heading) {
										myRC.moveBackward();
									}
									else if (myRC.getDirection() == heading.opposite()) {
										myRC.moveForward();
									}
									else {
										myRC.setDirection(heading.opposite());
									}
								}
								else {
									myRC.setDirection(heading);
								}
							}
							else {
								backPatience--;
							}
						}
						else {
							sidePatience--;
						}
					}
					else {
						diagonalPatience--;
					}
				}
				else {
					straightPatience--;
				}
			}
			
		}
		catch(Exception e) {
            System.out.println("caught exception:");
            e.printStackTrace();
        }
	}
	public void resetPatience() {
		//determines how long each robot type will wait for other robots to get out of the way in each status state when moving
		try {
			switch (myRC.getRobotType()) {
			case ARCHON:
				switch (myStatus) {
				case COMBAT: 
					straightPatience = 3; 
					diagonalPatience = 5;
					sidePatience = 5;
					backPatience = 5;
					break;
				case FINDHOME:
					straightPatience = 1; 
					diagonalPatience = 5;
					sidePatience = 10;
					backPatience = 10;
					break;
				default: //error unrecognized status
				}
			case CANNON:
				switch (myStatus) {
				case COMBAT: 
					straightPatience = 3; 
					diagonalPatience = 5;
					sidePatience = 5;
					backPatience = 5;
					break;
				default: //error unrecognized status
				}
			case CHANNELER:
				switch (myStatus) {
				case COMBAT: 
					straightPatience = 3; 
					diagonalPatience = 5;
					sidePatience = 5;
					backPatience = 5;
					break;
				default: //error unrecognized status
				}
			case SCOUT:
				switch (myStatus) {
				case COMBAT: 
					straightPatience = 3; 
					diagonalPatience = 5;
					sidePatience = 5;
					backPatience = 5;
					break;
				default: //error unrecognized status
				}
			case SOLDIER:
				switch (myStatus) {
				case COMBAT: 
					straightPatience = 3; 
					diagonalPatience = 5;
					sidePatience = 5;
					backPatience = 5;
					break;
				default: //error unrecognized status
				}
			case WORKER:
				switch (myStatus) {
				case COMBAT: 
					straightPatience = 3; 
					diagonalPatience = 5;
					sidePatience = 5;
					backPatience = 5;
					break;
				default: //error unrecognized status
				}
			default: //error unrecognized robot type
			}
		}catch(Exception e) {
            System.out.println("caught exception:");
            e.printStackTrace();
        }
	}
}